% Calculate the configuration variables of 2D knee point feet model
% StanceLegIndex: 1--right leg is the stance leg
%                 2--left leg is the stance leg
% torso roll angle cannot be fitted into cannonical functions
% 1. stance ankle roll angle
% 2. stance hip roll angle
% 3. non-stance hip roll angle

% modify the heigth of the hips
% data axis: x-walking direction y-height direction 
% model axis: x-walking direction z-height direction

function ret = TwoStep3D_angle(xpos,ypos,zpos,sns_brk,StanceLegIndex)


% ret = Knee3D_angle(zpos,ypos,xpos,sns_brk,StanceLegIndex);
% end
StartPt = sns_brk(1);
EndPt = sns_brk(4); % 1.5 step


%
for i = 1:length(xpos.lknee)
    
    model_torso = Data2Model(xpos.torso(i),ypos.torso(i),zpos.torso(i));
    model_hip_avg = Data2Model(xpos.hip_avg(i),ypos.hip_avg(i),zpos.hip_avg(i));
    model_lhip = Data2Model(xpos.lhip(i),ypos.lhip(i),zpos.lhip(i));
    model_lankle = Data2Model(xpos.lankle_avg(i), ypos.lankle_avg(i), zpos.lankle_avg(i));
    model_rhip = Data2Model(xpos.rhip(i),ypos.rhip(i),zpos.rhip(i));
    model_rankle = Data2Model(xpos.rankle_avg(i), ypos.rankle_avg(i), zpos.rankle_avg(i));
    
    % average hip height ; z direction;
    ave_hip = (model_lhip(3)+model_rhip(3))/2;
    model_lhip(3) = ave_hip;
    model_rhip(3) = ave_hip;
    
    % right leg is the stance leg
    if StanceLegIndex == 1
        if i==1
            init_sankle_roll = 0; %sankle_roll(model_rankle(2),model_rankle(3),model_rhip(2),model_rhip(3));
            init_ship_roll = 0; %ship_roll(model_rankle(2),model_rankle(3),model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
            init_nship_roll = 0; %nship_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
        end
        % stance ankle roll angle
        temp_sa = sankle_roll(model_rankle(2),model_rankle(3),model_rhip(2),model_rhip(3));
        sankle(i) = temp_sa  - init_sankle_roll; % the initial_sankle_roll is not zero
        
        % left hip roll angle
        lhip(i) = lhip_roll(model_lankle(2),model_lankle(3),...
            model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
        
        % right hip roll angle
        rhip(i) = rhip_roll(model_rankle(2),model_rankle(3),...
            model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
        
        % stance hip roll angle
        temp_ship = ship_roll(model_rankle(2),model_rankle(3),...
            model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
        ship(i) = temp_ship-init_ship_roll;
        
        % non-stance hip roll angle 
        temp_nship = nship_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
        nship(i) = temp_nship - init_nship_roll;
        
        % torso angle
        torso(i) = torso_roll(model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3),...
                              model_hip_avg(2),model_hip_avg(3),model_torso(2),model_torso(3));
    else % left leg is the stance leg
        % stance ankle roll angle
        if i==1
            init_sankle_roll = 0; %sankle_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3));
            init_ship_roll = 0; %ship_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
            init_nship_roll = 0; %nship_roll(model_rankle(2),model_rankle(3),model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
        end
        tempSA = sankle_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3));
        sankle(i) = -(tempSA-init_sankle_roll);
        % strance hip roll angle
        tempShip = ship_roll(model_lankle(2),model_lankle(3),model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
        ship(i) = tempShip - init_ship_roll;
        % non-stance hip roll angle
        tempNShip = nship_roll(model_rankle(2),model_rankle(3),model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
        nship(i) = tempNShip - init_nship_roll;
        % torso angle
       temp = torso_roll(model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3),...
                              model_hip_avg(2),model_hip_avg(3),model_torso(2),model_torso(3));
       torso(i) = temp;
       
               % left hip roll angle
        rhip(i) = lhip_roll(model_lankle(2),model_lankle(3),...
            model_lhip(2),model_lhip(3),model_rhip(2),model_rhip(3));
        
        % right hip roll angle
        lhip(i) = rhip_roll(model_rankle(2),model_rankle(3),...
            model_rhip(2),model_rhip(3),model_lhip(2),model_lhip(3));
    end
    
    % 
end

ret.lhip = lhip(StartPt:EndPt);
ret.rhip = rhip(StartPt:EndPt);

ret.sa =sankle(StartPt:EndPt);
ret.ship =ship(StartPt:EndPt);
ret.nship = nship(StartPt:EndPt);
ret.torso = torso(StartPt:EndPt);
end

% calculate the angle of the torso to the hip average
function ret = sankle_roll(yankle,zankle,yhip,zhip)
ret = atan((yhip-yankle)/(zhip-zankle));
end

% notes: positive x is the negative angle
% right leg is stance leg
function ret = ship_roll(ysankle,zsankle,yship,zship,ynship,znship)
pt1 = [0;ysankle;zsankle];
pt2 = [0;yship;zship];
pt3 = [0;ynship;znship];
ret = - pi/2 - ( -relAngleCos3D(pt2,pt1,pt2,pt3) );
end

% % notes: positive x is the positive angle
% % left leg is stance leg
% function ret = Lship_roll(ysankle,zsankle,yship,zship,ynship,znship)
% pt1 = [0;ysankle;zsankle];
% pt2 = [0;yship;zship];
% pt3 = [0;ynship;znship];
% ret = -pi/2-(-(relAngleCos3D(pt2,pt1,pt2,pt3) ));
% end

function ret=nship_roll(ynsankle,znsankle,ynship,znship,yship,zship)

ret = ship_roll(ynsankle,znsankle,ynship,znship,yship,zship);

end
% 
% 
% function ret=Lnship_roll(ynsankle,znsankle,ynship,znship,yship,zship)
% ret = Rship_roll(ynsankle,znsankle,ynship,znship,yship,zship);
% end



function ret = torso_roll(yship,zship,ynship,znship,yhip,zhip,ytorso,ztorso)
pt1 = [0;yship;zship];
pt2 = [0;ynship;znship];
pt3 = [0;yhip;zhip];
pt4 = [0;ytorso;ztorso];
ret = relAngleCos3D(pt1,pt2,pt3,pt4);
end


function ret = lhip_roll(ylankle,zlankle,ylhip,zlhip,yrhip,zrhip)
    ret = ship_roll(ylankle,zlankle,ylhip,zlhip,yrhip,zrhip);
end


function ret = rhip_roll(yrankle,zrankle,yrhip,zrhip,ylhip,zlhip)
    pt1 = [0;yrankle;zrankle];
pt2 = [0;yrhip;zrhip];
pt3 = [0;ylhip;zlhip];
ret = - pi/2 + (relAngleCos3D(pt2,pt1,pt2,pt3) );
%     ret = temp;
end